/**
 * File:   dofbot.c
 * Author: AWTK Develop Team
  * Brief:  dofbot c apis
 *
 * Copyright (c) 2025 - 2025 Guangzhou ZHIYUAN Electronics Co.,Ltd.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * License file for more details.
 *
 */

/**
 * History:
 * ================================================================
 * 2025-04-05 Li XianJing <xianjimli@hotmail.com> created
 *
 */

#include "dofbot.h"

#include "tkc/socket_helper.h"
#include "tkc/serial_helper.h"
#include "streams/serial/iostream_serial.h"

#define DOFBOT_MAGIC 0xFF
#define DOFBOT_DEVICE_ID 0xFC
#define DOFBOT_COMPLEMENT (257 - DOFBOT_DEVICE_ID)
#define DOFBOT_TIMEOUT_MS 10000
#define DOFBOT_JOINT_ID_OFFSET 0x30
#define DOFBOT_JOINT_ID(index) (DOFBOT_JOINT_ID_OFFSET + (index))

#define DOFBOT_FUNC_UART_SERVO 0x0A
#define DOFBOT_FUNC_UART_SUBS 0x0B
#define DOFBOT_FUNC_UART_NUM 0x22
#define DOFBOT_FUNC_UART_STATE 0x33
#define DOFBOT_FUNC_UART_VERSION 0x01
#define DOFBOT_FUNC_UART_RESULT 0x2A

/**
 * @class dofbot_packet_header_t
 * 包头信息，用于解析数据包。
 */
typedef struct _dofbot_packet_header_t {
  /**
   * @property {uint8_t} magic
   * magic(0xFF)
   */
  uint8_t magic;
  /**
   * @property {uint8_t} device_id
   * 设备ID(0xFC)
   */
  uint8_t device_id;
  /**
   * @property {uint8_t} size
   * 数据包大小
   */
  uint8_t size;
  /**
   * @property {uint8_t} type
   * 数据包类型
   */
  uint8_t type;
} dofbot_packet_header_t;

struct _dofbot_t {
  tk_iostream_t* ios;
  tk_istream_t* istream;
  tk_ostream_t* ostream;

  uint8_t buff[256];
  dofbot_packet_header_t resp_header;
};

static uint8_t* dofbot_pack_uint16(uint8_t* data, uint16_t value) {
  uint8_t* p = data;
  *p++ = (value >> 8) & 0xFF;
  *p++ = value & 0xFF;

  return p;
}

static uint8_t* dofbot_pack_uint8(uint8_t* data, uint8_t value) {
  uint8_t* p = data;
  *p++ = value;

  return p;
}

static int32_t dofbot_decode_angle(uint8_t id, int32_t d) {
  if (id == 5) {
    d = (int)((270 - 0) * (d - 380) / (3700 - 380) + 0);
    if (d > 270 || d < 0) {
      return 0;
    }
  } else {
    d = (int)((180 - 0) * (d - 900) / (3100 - 900) + 0);
    if (d > 180 || d < 0) {
      return 0;
    }
  }

  if (id == 2 || id == 3 || id == 4) {
    d = 180 - d;
  }

  return d;
}

static int32_t dofbot_encode_angle(uint8_t id, int32_t angle) {
  int32_t pos = 0;
  switch (id) {
    case 2:
    case 3:
    case 4: {
      angle = 180 - angle;
      pos = ((3100 - 900) * (angle - 0) / (180 - 0) + 900);
      break;
    }
    case 5: {
      pos = ((3700 - 380) * (angle - 0) / (270 - 0) + 380);
      break;
    }
    default: {
      pos = ((3100 - 900) * (angle - 0) / (180 - 0) + 900);
      break;
    }
  }

  return pos;
}

static ret_t dofbot_unpack_data(dofbot_t* dofbot, uint16_t* value, uint8_t* value_ext) {
  uint8_t type = 0;
  uint32_t data_size = 0;
  const uint8_t* data = NULL;
  return_value_if_fail(dofbot != NULL && dofbot->resp_header.size > 2, RET_BAD_PARAMS);
  data = dofbot->buff;
  type = dofbot->resp_header.type;
  data_size = dofbot->resp_header.size - 3;

  switch (type) {
    case DOFBOT_FUNC_UART_SERVO: {
      return_value_if_fail(data_size == 3, RET_BAD_PARAMS);
      *value = data[0] << 8 | data[1];
      if (value_ext != NULL) {
        *value_ext = data[2];
      }
      break;
    }
    case DOFBOT_FUNC_UART_SUBS: {
      return_value_if_fail(data_size == 2, RET_BAD_PARAMS);
      *value = data[0] << 8 | data[1];
      break;
    }
    case DOFBOT_FUNC_UART_VERSION: {
      return_value_if_fail(data_size == 1, RET_BAD_PARAMS);
      *value = data[0];
      break;
    }
    case DOFBOT_FUNC_UART_RESULT: {
      return_value_if_fail(data_size == 1, RET_BAD_PARAMS);
      *value = data[0];
      break;
    }
    default: {
      log_debug("not supported:%d\n", (int)type);
      return RET_FAIL;
    }
  }

  return RET_OK;
}

dofbot_t* dofbot_create(const char* serial_url) {
  dofbot_t* dofbot = NULL;
  tk_iostream_t* ios = NULL;
  return_value_if_fail(serial_url != NULL, NULL);
  ios = tk_iostream_serial_create(serial_url);
  return_value_if_fail(ios != NULL, NULL);
  dofbot = TKMEM_ZALLOC(dofbot_t);

  if (dofbot != NULL) {
    dofbot->ios = ios;
    dofbot->istream = tk_iostream_get_istream(ios);
    dofbot->ostream = tk_iostream_get_ostream(ios);

    sleep_ms(500);
  } else {
    TK_OBJECT_UNREF(ios);
  }

  return dofbot;
}

static uint8_t dofbot_sum(const uint8_t* data, size_t len, uint32_t start) {
  uint32_t sum = start;

  for (size_t i = 0; i < len; i++) {
    sum += data[i];
  }

  return sum & 0xFF;
}

static ret_t dofbot_append_sum(uint8_t* data, size_t len, uint32_t start) {
  uint32_t data_len = len - 1;
  return_value_if_fail(len > 1 && data != NULL, RET_BAD_PARAMS);

  data[data_len] = dofbot_sum(data, data_len, start);

  return RET_OK;
}

static ret_t dofbot_send_cmd(dofbot_t* dofbot, uint8_t* cmd, uint32_t cmd_len) {
  return_value_if_fail(dofbot != NULL && cmd != NULL, RET_BAD_PARAMS);

  cmd[2] = (uint8_t)cmd_len - 2;
  dofbot_append_sum(cmd, cmd_len, DOFBOT_COMPLEMENT);

  return tk_ostream_write(dofbot->ostream, cmd, cmd_len) == cmd_len ? RET_OK : RET_IO;
}

static int32_t dofbot_recv_data(dofbot_t* dofbot, void* buff, uint32_t buff_len) {
  return tk_istream_read_len(dofbot->istream, buff, buff_len, DOFBOT_TIMEOUT_MS);
}

static ret_t dofbot_recv_resp_header(dofbot_t* dofbot, dofbot_packet_header_t* header) {
  uint8_t magic = 0;
  uint8_t device_id = 0;
  uint8_t size = 0;
  uint8_t type = 0;
  int32_t ret = 0;

  do {
    ret = dofbot_recv_data(dofbot, &magic, 1);
    return_value_if_fail(ret == 1, RET_FAIL);
    if (magic == DOFBOT_MAGIC) {
      break;
    }
  } while(1);

  do {
    ret = dofbot_recv_data(dofbot, &device_id, 1);
    return_value_if_fail(ret == 1, RET_FAIL);
    if ((device_id + 1) == DOFBOT_DEVICE_ID) {
      break;
    }
  } while(1);

  ret = dofbot_recv_data(dofbot, &size, 1);
  return_value_if_fail(ret == 1, RET_FAIL);

  ret = dofbot_recv_data(dofbot, &type, 1);
  return_value_if_fail(ret == 1, RET_FAIL);

  header->magic = magic;
  header->device_id = device_id;
  header->size = size;
  header->type = type;

  return_value_if_fail(header->size > 0, RET_BAD_PARAMS);
  return_value_if_fail((size_t)(header->size) < sizeof(dofbot->buff), RET_BAD_PARAMS);

  return RET_OK;
}

static ret_t dofbot_recv_resp(dofbot_t* dofbot) {
  int32_t ret = 0;
  uint8_t check_sum = 0;
  uint32_t check_sum_start = 0;
  int32_t size = sizeof(dofbot->resp_header);
  dofbot_packet_header_t* header = &dofbot->resp_header;

  memset(header, 0x00, sizeof(*header));
  return_value_if_fail(dofbot_recv_resp_header(dofbot, header) == RET_OK, RET_FAIL);

  memset(dofbot->buff, 0x00, sizeof(dofbot->buff));
  size = header->size - 2;
  check_sum_start = header->size + header->type;
  ret = dofbot_recv_data(dofbot, dofbot->buff, size);
  return_value_if_fail(ret == size, RET_IO);
  check_sum = dofbot_sum(dofbot->buff, size - 1, check_sum_start);

  return check_sum == dofbot->buff[size - 1] ? RET_OK : RET_IO;
}

static uint8_t* dofbot_pack_header(uint8_t* data, uint8_t cmd) {
  uint8_t* p = data;
  return_value_if_fail(data != NULL, NULL);

  *p++ = DOFBOT_MAGIC;
  *p++ = DOFBOT_DEVICE_ID;
  *p++ = 0;
  *p++ = cmd;

  return p;
}

ret_t dofbot_write_servos_array(dofbot_t* dofbot, const int32_t values[6], int32_t time) {
  return dofbot_write_servos(dofbot, values[0], values[1], values[2], values[3], values[4], values[5], time);
}

ret_t dofbot_write_servos(dofbot_t* dofbot, int32_t s1, int32_t s2, int32_t s3, int32_t s4,
                          int32_t s5, int32_t s6, int32_t time) {
  uint16_t pos = 0;
  uint8_t* p = NULL;
  uint8_t* data = NULL;
  return_value_if_fail(dofbot != NULL, RET_BAD_PARAMS);

  p = dofbot->buff;
  data = dofbot->buff;
  p = dofbot_pack_header(p, 0x1d);

  pos = dofbot_encode_angle(1, s1);
  p = dofbot_pack_uint16(p, pos);

  pos = dofbot_encode_angle(2, s2);
  p = dofbot_pack_uint16(p, pos);

  pos = dofbot_encode_angle(3, s3);
  p = dofbot_pack_uint16(p, pos);

  pos = dofbot_encode_angle(4, s4);
  p = dofbot_pack_uint16(p, pos);

  pos = dofbot_encode_angle(5, s5);
  p = dofbot_pack_uint16(p, pos);

  pos = dofbot_encode_angle(6, s6);
  p = dofbot_pack_uint16(p, pos);

  p = dofbot_pack_uint16(p, time);

  return dofbot_send_cmd(dofbot, data, p - data + 1);
}

ret_t dofbot_write_servo(dofbot_t* dofbot, uint8_t id, int32_t angle, int32_t time) {
  return_value_if_fail(dofbot != NULL, RET_BAD_PARAMS);

  if (id == 0) {
    return dofbot_write_servos(dofbot, angle, angle, angle, angle, angle, angle, time);
  } else {
    uint8_t* p = dofbot->buff;
    uint8_t* data = dofbot->buff;
    uint16_t pos = dofbot_encode_angle(id, angle);

    p = dofbot_pack_header(p, 0x10 + id);
    p = dofbot_pack_uint16(p, pos);
    p = dofbot_pack_uint16(p, time);

    return dofbot_send_cmd(dofbot, data, p - data + 1);
  }
}

ret_t dofbot_read_servo(dofbot_t* dofbot, uint8_t id, int32_t* value) {
  int32_t i = 0;
  ret_t ret = RET_FAIL;
  uint8_t ret_id = 0;
  uint16_t ret_value = 0;
  uint8_t* p = NULL;
  uint8_t* data = NULL;
  return_value_if_fail(dofbot != NULL, RET_BAD_PARAMS);
  return_value_if_fail(id > 0 && id < 7, RET_BAD_PARAMS);
  return_value_if_fail(dofbot != NULL && value != NULL, RET_BAD_PARAMS);

  /*FIXME: 不清楚 Arm_Lib.py 里为什么要读两次*/
  for (i = 0; i < 2; i++) {
    p = dofbot->buff;
    data = dofbot->buff;

    p = dofbot_pack_header(p, DOFBOT_JOINT_ID(id));
    ret = dofbot_send_cmd(dofbot, data, p - data + 1);
    return_value_if_fail(ret == RET_OK, RET_BAD_PARAMS);
    return_value_if_fail(dofbot_recv_resp(dofbot) == RET_OK, RET_BAD_PARAMS);

    dofbot_unpack_data(dofbot, &ret_value, &ret_id);
    return_value_if_fail(ret_id == DOFBOT_JOINT_ID(id), RET_BAD_PARAMS);
  }

  *value = dofbot_decode_angle(id, ret_value);

  return RET_OK;
}

ret_t dofbot_read_version(dofbot_t* dofbot, int32_t* value) {
  ret_t ret = RET_FAIL;
  uint16_t ret_value = 0;
  uint8_t* p = NULL;
  uint8_t* data = NULL;
  return_value_if_fail(dofbot != NULL && value != NULL, RET_BAD_PARAMS);

  p = dofbot->buff;
  data = dofbot->buff;

  p = dofbot_pack_header(p, 0x01);
  ret = dofbot_send_cmd(dofbot, data, p - data + 1);
  return_value_if_fail(ret == RET_OK, RET_BAD_PARAMS);
  return_value_if_fail(dofbot_recv_resp(dofbot) == RET_OK, RET_BAD_PARAMS);

  dofbot_unpack_data(dofbot, &ret_value, NULL);
  *value = ret_value;

  return RET_OK;
}

static ret_t dofbot_send_set_cmd(dofbot_t* dofbot, const uint8_t* cmd_and_args, uint32_t size) {
  uint32_t i = 0;
  uint8_t* p = NULL;
  uint8_t* data = NULL;
  return_value_if_fail(dofbot != NULL, RET_BAD_PARAMS);
  return_value_if_fail(cmd_and_args != NULL && size > 0, RET_BAD_PARAMS);

  p = dofbot->buff;
  data = dofbot->buff;
  p = dofbot_pack_header(p, cmd_and_args[0]);

  for (i = 1; i < size; i++) {
    p = dofbot_pack_uint8(p, cmd_and_args[i]);
  }

  return dofbot_send_cmd(dofbot, data, p - data + 1);
}

ret_t dofbot_reset(dofbot_t* dofbot) {
  uint8_t cmd_and_args[] = {0x05};
  return dofbot_send_set_cmd(dofbot, cmd_and_args, sizeof(cmd_and_args));
}

ret_t dofbot_set_rgb(dofbot_t* dofbot, uint8_t r, uint8_t g, uint8_t b) {
  uint8_t cmd_and_args[] = {0x02, r, g, b};
  return dofbot_send_set_cmd(dofbot, cmd_and_args, sizeof(cmd_and_args));
}

static const int s_home_position[] = {90, 90, 90, 90, 90, 180};

ret_t dofbot_home(dofbot_t* dofbot) {
/*
 //内置命令似乎没有实现
 //uint8_t cmd_and_args[] = {0x1b};
 //return dofbot_send_set_cmd(dofbot, cmd_and_args, sizeof(cmd_and_args));
 */
  return dofbot_write_servos_array(dofbot, s_home_position, 2000);
}

ret_t dofbot_home_servo(dofbot_t* dofbot, uint8_t id) {
  return_value_if_fail(id < ARRAY_SIZE(s_home_position), RET_BAD_PARAMS);
/*
 //内置命令似乎没有实现
  uint8_t cmd_and_args[] = {0x1c, id};
  return dofbot_send_set_cmd(dofbot, cmd_and_args, sizeof(cmd_and_args));
 */
  return dofbot_write_servo(dofbot, id, s_home_position[id], 2000);
}

ret_t dofbot_buzzer(dofbot_t* dofbot, uint8_t duration) {
  uint8_t cmd_and_args[] = {0x06, duration};
  return dofbot_send_set_cmd(dofbot, cmd_and_args, sizeof(cmd_and_args));
}

ret_t dofbot_set_torque_mode(dofbot_t* dofbot, bool_t on) {
  uint8_t cmd_and_args[] = {0x1a, on ? 1 : 0};
  return dofbot_send_set_cmd(dofbot, cmd_and_args, sizeof(cmd_and_args));
}

ret_t dofbot_destroy(dofbot_t* dofbot) {
  return_value_if_fail(dofbot != NULL, RET_BAD_PARAMS);
  TK_OBJECT_UNREF(dofbot->ios);
  TKMEM_FREE(dofbot);

  return RET_OK;
}

